#include "stm32f4xx.h"                  // Device header
#include "PID/PID.h"
//#include "Serial.h"
#include "stdio.h"



void PID_V_Init(PID* pid,float kp,float ki,float kd)
{
	pid->V.Kp = kp;
	pid->V.Ki = ki;
	pid->V.Kd = kd;
	pid->V.error_now = 0;
	pid->V.error_last = 0;
	pid->V.error_last_l = 0;
	pid->V.Target = 0;
	pid->V.mean = 0;
}

void PID_A_Init(PID* pid,float kp,float ki,float kd)
{
	pid->P.Kp = kp;
	pid->P.Ki = ki;
	pid->P.Kd = kd;
	pid->P.error_now = 0;
	pid->P.error_last = 0;
	pid->P.Integral = 0;
	pid->P.Target = 0;
}

float updataPID_V(PID* pid,float input)
{
	pid->V.error_last_l = pid->V.error_last;
	pid->V.error_last = pid->V.error_now;
	pid->V.error_now = pid->V.Target - input;
	pid->V.mean += (pid->V.Kp*(pid->V.error_now-pid->V.error_last))
		 +(pid->V.Ki*pid->V.error_now)
		 +(pid->V.Kd*(pid->V.error_now-2*pid->V.error_last+pid->V.error_last_l));
//	printf("%.2f,%.2f,%.2f,%.2f,%.2f\n",(pid->V.Kp),(pid->V.Ki),(pid->V.Kd),(pid->V.Target),input);
//	printf("%d,%d,%d,%d,%d\n",(int)(pid->V.error_now),(int)(pid->V.error_last),(int)(pid->V.error_last_l),(int)(pid->V.Target),(int)input);
	return pid->V.mean;
}

float updataPID_A(PID* pid,float input)
{
	pid->P.error_last = pid->P.error_now;
	pid->P.error_now = pid->P.Target - input;
	pid->P.Integral += pid->P.error_now;	
//	printf("%d,%d,%d,%d,%d\n",(int)(input),(int)(A_Target),(int)(A_Kp_Left*100),(int)(A_Ki_Left*100),(int)(A_Kd_Left*100));
	if(pid->P.Integral>=166)
	pid->P.Integral = 166;
	if(pid->P.Integral<=-166)
	pid->P.Integral = -166;
	
	return pid->P.Kp * pid->P.error_now
		+ pid->P.Ki * pid->P.Integral 
		+ pid->P.Kd * (pid->P.error_now - pid->P.error_last);
}

void Set_PID_V_Target(PID* pid,float target)
{
	pid->V.Target = target;
}

void Set_PID_A_Target(PID* pid,float target)
{
	pid->P.Target = target;
}
